/***************************************************************************
 创建者: 华磊
 开始时间: 2016.11.28
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

华磊，2017.6.6
１）将sixrobot的求解函数变成可重入，在函数内部实例化sixrobot进行使用。
２）测试ＫＤＬ的ik迭代求解方法，证明可行，求解速度快，数值准确。
 ***************************************************************************
 *  @file coordinate.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef COORDINATE_H
#define COORDINATE_H

#include "domparser.h"
#include "GeneralDefine.h"
#include "pointdata.h"
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolvervel_pinv_givens.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl/frames.hpp>
#include "sixjointrobot.h"
#include "messagelog.h"
#include "datarecord.h"
#include "motionmessage.h"
#include <QMutex>
#include "DeltaKinematics.h"
#include "DeltaKinematicsKe.h"
#include "decartwrist.h"
#include "ur_kin.h"
#include "genhexkins.h"

using namespace KDL;
class FiveAxisKinematic;
/**
 * @brief The CoordinateSolver class 通用正解和逆向的求解
 */
class CoordinateSolver:  public DomParser
{

public:
    CoordinateSolver(int robotTypeIn, int internalNumIn, int robotIdIn,
                     QVector<JointAttribute> *robotAttributeIn, MotionMessage * motionMessageIn
                     ,QVector<double> gravityIn);
    ~CoordinateSolver();

public:
    /**
     * @brief getBestPositionFromAngle
     * @param idealAngle -180~180
     * @param initialPosition 包含多圈的绝对角度
     * @param bestPositionOut　包含多圈的绝对角度
     * @return
     */
    int getBestPositionFromAngle(double idealAngle,double initialPosition,double &bestPositionOut);
    int setGravityVector(vector<double> gravityIn);
    int setToolMass(double mass,vector<double> center);
    int setSegmentAttribute(int index,JointAttribute attributeIn);
    int pointCoordinateToCartesian(const PointPro pointIn,PointPro &pointOut);
    int pointCoordinateToJoint(const PointPro pointIn, vector<double> positionInitialList,
                               PointPro &jointPointOut,bool isErrorNotice=true);

    int pointProToFrame_noUT(const PointPro& pointPro, Frame &frameOut);
    PointPro frameToPointPro_CART(const Frame& frameIn);//华磊增加多解选取信息,丢失了点号外部轴数据
    /**
     * @brief frameToPointPro_JOINT
     * @param frameIn
     * @param pointOut
     * @param configString
     * @param positionInitialList 单位mm degree
     * @return
     */
    int frameToPointPro_JOINT(const Frame& frameIn, PointPro &pointOut, bool &isSinguratyOut, string configString
                              , vector<double> positionInitialList, bool isErrorNotice=true);//pointOut 里面含有多解选择string
    int frameToPointPro_JOINT(const Frame& frameIn, PointPro &pointOut, string configString);//不考虑和初始位置的偏差检查

    int getIdealJointTorque(const JntArray& q_in,  JntArray tmpQdot, JntArray tmpQdotdot,
                            Wrench f_tool, JntArray &torqueOut);
    int getFkPos(const JntArray& q_in, Frame& pos);
    int getFkVel(const JntArrayVel& qvel_in, FrameVel& vel);

    int getIkPos_quick_fut(QVector<int> futConfig, const Frame& p_in, JntArray& joints, bool &isSinguratyOut);
    int getIkPos_quick(const JntArray& q_init, const Frame& p_in, JntArray& joints, bool &isSinguratyOut);
    int getIkPos_iteration(const JntArray& q_init, const Frame& p_in, JntArray& joints);

    int getIdSolver_RNE(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext, JntArray &torques);
    int getGravity(const JntArray &q, JntArray &gravity);

    int getMultiConfig(const std::vector<double> positionValue, std::string &configString, std::vector<int> &configInt);//unit: mm degree
    int getMultiConfig_UR(const std::vector<double> positionValue, std::string &configString,
                          std::vector<int> &configInt);
    int getMultiConfig_XYZR(const std::vector<double> positionValue, std::string &configString,
                          std::vector<int> &configInt);
    int getMultiConfig_FANUC(const std::vector<double> positionValue, std::string &configString,
                          std::vector<int> &configInt);
    int mRad2mmDegree(const JntArray& mRad, vector<double> &mmDegree);
    int mRad2mmDegree(vector<double> mRad, vector<double> &mmDegree);
    int mmDegree2mRad(vector<double> mmDegree, vector<double> &mRad);

    Rotation createEulerMatrix(double psi, double sita, double fai);
    void extractEulerMatrixAngle(const Rotation& rotMat, double& psi, double& sita, double& fai);


private:
    void initRobotChain();
    void initRobotChain_chain(Chain &robotChainOut);
    int transConfigString2ConfigInt(QString configString, QVector<int> &futConfig);
    int transConfigString2ConfigInt_UR(QString configString, QVector<int> &futConfig);
    int transConfigString2ConfigInt_XYZR(QString configString, QVector<int> &futConfig);
    int transConfigString2ConfigInt_FANUC(QString configString, QVector<int> &futConfig);
    int checkIkSolverJointOffset(const JntArray& q_init, const JntArray& outputJoints);


    Frame jntArrayToFrame(const JntArray& jntArray);
    int getIkPos_NR(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_LMA(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_NR_JL(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkVel_pinv(const JntArray& q_in, const Twist& v_in, JntArray& jointsDot);
    int getIkVel_pinvGivens(const JntArray& q_in, const Twist& v_in, JntArray& jointsDot);
    int getIkPos_SCARA(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_SCARA_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints);
    int getIkPos_descart( const Frame& p_in, JntArray& joints);
    int getFkPos_descart(const JntArray& q_in, Frame& pos);
    int getIkPos_descart_XYZR(const JntArray &q_initIn, const Frame& p_in, JntArray& joints);
    int getFkPos_descart_XYZR(const JntArray& q_in, Frame& pos);
    int getIkPos_descart_XYZR_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints);
   // int getIkPos_PALLET(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    //int getIkPos_ADAPT(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_SIX(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_SIX_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints);
    int getIkPos_decartwrist(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_decartwrac(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_decartwrist_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints);
    int getIkPos_decartwrac_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints);
    //delta
    int getIkPos_delta(const JntArray& q_init, const Frame& p_in, JntArray& joints);
    int getIkPos_delta_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints);
    int getFkPos_delta(const JntArray& q_in, Frame& p_out);
    //ur robot
    int getIkPos_ur(const JntArray &q_init, const Frame& p_in, JntArray& joints, bool &isSinguratyOut);
    int getIkPos_ur_FUT(QVector<int> futConfig, const Frame& p_in, JntArray& joints, bool &isSinguratyOut);
    int getFkPos_ur(const JntArray& q_in, Frame& p_out);

    //HEXPOD robot
    int getIkPos_hexpod(const Frame& p_in, JntArray& jointsOut);
    int getFkPos_hexpod(const JntArray& q_in, Frame& p_out);

    int getTurnsFromDegree(double degreeIn);
    int getTurnsFromRad(double degreeIn);
    double getTrimDegreeFromDegree(double degreeIn);//not used
    double getTrimRadFromRad(double degreeIn);//not used

    int getBestPositionFromAngle_Rad(double idealAngle,double initialPosition,double &bestPositionOut);
    int initialDeltaDhParameter(QVector<JointAttribute> *robotAttributeIn);
    int initialUrrobotDhParameter(QVector<JointAttribute> *robotAttributeIn);
    int initialHexpodDhParameter(QVector<JointAttribute> *robotAttributeIn);
    double transformToPi(double angleIn);

    void addMsg(int messageLevel,
                std::string componentName,
                std::string messageType,
                int messageCode,
                int robotIdIn,
                int parameter1 = 0,
                int parameter2 = 0,
                int parameter3 = 0,
                int parameter4 = 0);

private:
    /**
     * @brief robotType, 机器人类型
     */
    int robotType;
    int internalNum;//内部轴数量
    int robotId;
    QVector<JointAttribute> *robotJoints;
    Chain robotChain;
    Vector gravityVector;
    MotionMessage *motionMessage;
    JntArray jointsMax;//关节正限位
    JntArray jointsMin;//关节负限位
    ChainFkSolverPos_recursive *fkSolverPos;
    ChainFkSolverVel_recursive *fkSolverVel;
    ChainIkSolverVel_pinv *ikSolverVel_pinv;
    ChainIkSolverVel_pinv_givens *ikSolverVel_pinvGivens;
    ChainIkSolverPos_LMA *ikSolverPos_LMA;
    ChainIkSolverPos_NR *ikSolverPos_NR;
    ChainIkSolverPos_NR_JL *ikSolverPos_NR_JL;
    ChainIdSolver_RNE *idSolver_RNE;
    ChainDynParam *dynParam;
    DataRecord* dataRecord;

    double robotDhParameter[6][4];//目前仅六轴使用
//    DeltaKinematics<double>::DeltaGeometricDim deltaDhParameter;
//    DeltaKinematics<double> *deltaRobotKinetic;
    DeltaKinematicsKe *deltaRobotKinetic;
//    QMutex usingMutex;//解决多线程重入问题
    UrKinematics *urKinematic;
    HexPodKinematics *hexpodKin;

    float origin_mass;
    QVector<float> origin_massCenter;
    FiveAxisKinematic* fiveAxisKin;

};

#endif // COORDINATE_H
